#include "dcmath.h"

#include <math.h>

//See http://forum.43oh.com/topic/7119-issue-with-sqrt/
int __errno;

//In this library, we pass in / pass out angles in radians.  Internally, we use a custom measurement which maps
// 90 degrees (PI / 2 radians) into 256 segments (hereafter this unit is called a 'segment', or 'seg').

//Generated with:
//    for (float i = -1; i <= 1; i+=0.01){ printf("%1.5ff, ", acos(i)); }
// (add a 0 at the end)
static float lookup_acos[201] = {
	3.14159f, 3.00005f, 2.94126f, 2.89603f, 2.85780f, 2.82403f, 2.79343f, 2.76521f, 2.73888f, 2.71408f, 2.69057f, 2.66814f, 2.64666f, 2.62600f, 2.60607f, 2.58678f, 2.56808f, 2.54990f, 2.53221f, 2.51495f, 2.49809f, 2.48161f, 2.46546f, 2.44964f, 2.43411f, 2.41886f, 2.40387f, 2.38912f, 2.37460f, 2.36029f, 2.34619f, 2.33229f, 2.31856f, 2.30501f, 2.29162f, 2.27838f, 2.26529f, 2.25235f, 2.23954f, 2.22686f, 2.21430f, 2.20186f, 2.18953f, 2.17730f, 2.16518f, 2.15316f, 2.14123f, 2.12940f, 2.11765f, 2.10598f, 2.09440f, 2.08289f, 2.07145f, 2.06009f, 2.04879f, 2.03756f, 2.02640f, 2.01529f, 2.00424f, 1.99325f, 1.98231f, 1.97143f, 1.96059f, 1.94981f, 1.93906f, 1.92837f, 1.91771f, 1.90710f, 1.89653f, 1.88599f, 1.87549f, 1.86502f, 1.85459f, 1.84419f, 1.83382f, 1.82348f, 1.81316f, 1.80287f, 1.79261f, 1.78237f, 1.77215f, 1.76196f, 1.75178f, 1.74163f, 1.73149f, 1.72136f, 1.71126f, 1.70117f, 1.69109f, 1.68102f, 1.67096f, 1.66092f, 1.65088f, 1.64085f, 1.63083f, 1.62082f, 1.61081f, 1.60080f, 1.59080f, 1.58080f, 1.57080f, 1.56080f, 1.55079f, 1.54079f, 1.53079f, 1.52078f, 1.51076f, 1.50074f, 1.49071f, 1.48067f, 1.47063f, 1.46057f, 1.45051f, 1.44043f, 1.43033f, 1.42023f, 1.41011f, 1.39997f, 1.38981f, 1.37963f, 1.36944f, 1.35922f, 1.34898f, 1.33872f, 1.32843f, 1.31812f, 1.30777f, 1.29740f, 1.28700f, 1.27657f, 1.26610f, 1.25560f, 1.24507f, 1.23449f, 1.22388f, 1.21323f, 1.20253f, 1.19179f, 1.18100f, 1.17016f, 1.15928f, 1.14834f, 1.13735f, 1.12630f, 1.11520f, 1.10403f, 1.09280f, 1.08151f, 1.07014f, 1.05871f, 1.04720f, 1.03561f, 1.02395f, 1.01220f, 1.00036f, 0.98843f, 0.97641f, 0.96429f, 0.95207f, 0.93974f, 0.92730f, 0.91474f, 0.90205f, 0.88924f, 0.87630f, 0.86321f, 0.84998f, 0.83659f, 0.82303f, 0.80931f, 0.79540f, 0.78130f, 0.76699f, 0.75247f, 0.73773f, 0.72273f, 0.70748f, 0.69196f, 0.67613f, 0.65999f, 0.64350f, 0.62664f, 0.60939f, 0.59169f, 0.57351f, 0.55481f, 0.53553f, 0.51559f, 0.49493f, 0.47345f, 0.45103f, 0.42751f, 0.40272f, 0.37638f, 0.34817f, 0.31756f, 0.28379f, 0.24557f, 0.20033f, 0.14154f, 0.00000f
};

//Generated with:
//    for (uint16_t i = 0; i <= 255; i++){ printf("%1.5ff, ", cos(i * M_PI / 512)); }
// (add a 0 at the end)
static float lookup_cos[257] = {
	1.00000f, 0.99998f, 0.99992f, 0.99983f, 0.99970f, 0.99953f, 0.99932f, 0.99908f, 0.99880f, 0.99848f, 0.99812f, 0.99772f, 0.99729f, 0.99682f, 0.99631f, 0.99577f, 0.99518f, 0.99456f, 0.99391f, 0.99321f, 0.99248f, 0.99171f, 0.99090f, 0.99006f, 0.98918f, 0.98826f, 0.98730f, 0.98631f, 0.98528f, 0.98421f, 0.98311f, 0.98196f, 0.98079f, 0.97957f, 0.97832f, 0.97703f, 0.97570f, 0.97434f, 0.97294f, 0.97150f, 0.97003f, 0.96852f, 0.96698f, 0.96539f, 0.96378f, 0.96212f, 0.96043f, 0.95870f, 0.95694f, 0.95514f, 0.95331f, 0.95144f, 0.94953f, 0.94759f, 0.94561f, 0.94359f, 0.94154f, 0.93946f, 0.93734f, 0.93518f, 0.93299f, 0.93077f, 0.92851f, 0.92621f, 0.92388f, 0.92151f, 0.91911f, 0.91668f, 0.91421f, 0.91171f, 0.90917f, 0.90660f, 0.90399f, 0.90135f, 0.89867f, 0.89597f, 0.89322f, 0.89045f, 0.88764f, 0.88480f, 0.88192f, 0.87901f, 0.87607f, 0.87309f, 0.87009f, 0.86705f, 0.86397f, 0.86087f, 0.85773f, 0.85456f, 0.85136f, 0.84812f, 0.84485f, 0.84155f, 0.83822f, 0.83486f, 0.83147f, 0.82805f, 0.82459f, 0.82110f, 0.81758f, 0.81404f, 0.81046f, 0.80685f, 0.80321f, 0.79954f, 0.79584f, 0.79211f, 0.78835f, 0.78456f, 0.78074f, 0.77689f, 0.77301f, 0.76910f, 0.76517f, 0.76120f, 0.75721f, 0.75319f, 0.74914f, 0.74506f, 0.74095f, 0.73682f, 0.73265f, 0.72846f, 0.72425f, 0.72000f, 0.71573f, 0.71143f, 0.70711f, 0.70275f, 0.69838f, 0.69397f, 0.68954f, 0.68508f, 0.68060f, 0.67609f, 0.67156f, 0.66700f, 0.66242f, 0.65781f, 0.65317f, 0.64851f, 0.64383f, 0.63912f, 0.63439f, 0.62964f, 0.62486f, 0.62006f, 0.61523f, 0.61038f, 0.60551f, 0.60062f, 0.59570f, 0.59076f, 0.58580f, 0.58081f, 0.57581f, 0.57078f, 0.56573f, 0.56066f, 0.55557f, 0.55046f, 0.54532f, 0.54017f, 0.53500f, 0.52980f, 0.52459f, 0.51936f, 0.51410f, 0.50883f, 0.50354f, 0.49823f, 0.49290f, 0.48755f, 0.48218f, 0.47680f, 0.47140f, 0.46598f, 0.46054f, 0.45508f, 0.44961f, 0.44412f, 0.43862f, 0.43309f, 0.42756f, 0.42200f, 0.41643f, 0.41084f, 0.40524f, 0.39962f, 0.39399f, 0.38835f, 0.38268f, 0.37701f, 0.37132f, 0.36561f, 0.35990f, 0.35416f, 0.34842f, 0.34266f, 0.33689f, 0.33111f, 0.32531f, 0.31950f, 0.31368f, 0.30785f, 0.30201f, 0.29615f, 0.29028f, 0.28441f, 0.27852f, 0.27262f, 0.26671f, 0.26079f, 0.25487f, 0.24893f, 0.24298f, 0.23702f, 0.23106f, 0.22508f, 0.21910f, 0.21311f, 0.20711f, 0.20110f, 0.19509f, 0.18907f, 0.18304f, 0.17700f, 0.17096f, 0.16491f, 0.15886f, 0.15280f, 0.14673f, 0.14066f, 0.13458f, 0.12850f, 0.12241f, 0.11632f, 0.11022f, 0.10412f, 0.09802f, 0.09191f, 0.08580f, 0.07968f, 0.07356f, 0.06744f, 0.06132f, 0.05520f, 0.04907f, 0.04294f, 0.03681f, 0.03067f, 0.02454f, 0.01841f, 0.01227f, 0.00614f, 0.00000f
};

//Generated with:
//    for (uint16_t i = 0; i <= 255; i++){ printf("%1.5ff, ", sin(i * M_PI / 512)); }
// (add a 1 at the end)
static float lookup_sin[257] = {
	0.00000f, 0.00614f, 0.01227f, 0.01841f, 0.02454f, 0.03067f, 0.03681f, 0.04294f, 0.04907f, 0.05520f, 0.06132f, 0.06744f, 0.07356f, 0.07968f, 0.08580f, 0.09191f, 0.09802f, 0.10412f, 0.11022f, 0.11632f, 0.12241f, 0.12850f, 0.13458f, 0.14066f, 0.14673f, 0.15280f, 0.15886f, 0.16491f, 0.17096f, 0.17700f, 0.18304f, 0.18907f, 0.19509f, 0.20110f, 0.20711f, 0.21311f, 0.21910f, 0.22508f, 0.23106f, 0.23702f, 0.24298f, 0.24893f, 0.25487f, 0.26079f, 0.26671f, 0.27262f, 0.27852f, 0.28441f, 0.29028f, 0.29615f, 0.30201f, 0.30785f, 0.31368f, 0.31950f, 0.32531f, 0.33111f, 0.33689f, 0.34266f, 0.34842f, 0.35416f, 0.35990f, 0.36561f, 0.37132f, 0.37701f, 0.38268f, 0.38835f, 0.39399f, 0.39962f, 0.40524f, 0.41084f, 0.41643f, 0.42200f, 0.42756f, 0.43309f, 0.43862f, 0.44412f, 0.44961f, 0.45508f, 0.46054f, 0.46598f, 0.47140f, 0.47680f, 0.48218f, 0.48755f, 0.49290f, 0.49823f, 0.50354f, 0.50883f, 0.51410f, 0.51936f, 0.52459f, 0.52980f, 0.53500f, 0.54017f, 0.54532f, 0.55046f, 0.55557f, 0.56066f, 0.56573f, 0.57078f, 0.57581f, 0.58081f, 0.58580f, 0.59076f, 0.59570f, 0.60062f, 0.60551f, 0.61038f, 0.61523f, 0.62006f, 0.62486f, 0.62964f, 0.63439f, 0.63912f, 0.64383f, 0.64851f, 0.65317f, 0.65781f, 0.66242f, 0.66700f, 0.67156f, 0.67609f, 0.68060f, 0.68508f, 0.68954f, 0.69397f, 0.69838f, 0.70275f, 0.70711f, 0.71143f, 0.71573f, 0.72000f, 0.72425f, 0.72846f, 0.73265f, 0.73682f, 0.74095f, 0.74506f, 0.74914f, 0.75319f, 0.75721f, 0.76120f, 0.76517f, 0.76910f, 0.77301f, 0.77689f, 0.78074f, 0.78456f, 0.78835f, 0.79211f, 0.79584f, 0.79954f, 0.80321f, 0.80685f, 0.81046f, 0.81404f, 0.81758f, 0.82110f, 0.82459f, 0.82805f, 0.83147f, 0.83486f, 0.83822f, 0.84155f, 0.84485f, 0.84812f, 0.85136f, 0.85456f, 0.85773f, 0.86087f, 0.86397f, 0.86705f, 0.87009f, 0.87309f, 0.87607f, 0.87901f, 0.88192f, 0.88480f, 0.88764f, 0.89045f, 0.89322f, 0.89597f, 0.89867f, 0.90135f, 0.90399f, 0.90660f, 0.90917f, 0.91171f, 0.91421f, 0.91668f, 0.91911f, 0.92151f, 0.92388f, 0.92621f, 0.92851f, 0.93077f, 0.93299f, 0.93518f, 0.93734f, 0.93946f, 0.94154f, 0.94359f, 0.94561f, 0.94759f, 0.94953f, 0.95144f, 0.95331f, 0.95514f, 0.95694f, 0.95870f, 0.96043f, 0.96212f, 0.96378f, 0.96539f, 0.96698f, 0.96852f, 0.97003f, 0.97150f, 0.97294f, 0.97434f, 0.97570f, 0.97703f, 0.97832f, 0.97957f, 0.98079f, 0.98196f, 0.98311f, 0.98421f, 0.98528f, 0.98631f, 0.98730f, 0.98826f, 0.98918f, 0.99006f, 0.99090f, 0.99171f, 0.99248f, 0.99321f, 0.99391f, 0.99456f, 0.99518f, 0.99577f, 0.99631f, 0.99682f, 0.99729f, 0.99772f, 0.99812f, 0.99848f, 0.99880f, 0.99908f, 0.99932f, 0.99953f, 0.99970f, 0.99983f, 0.99992f, 0.99998f, 1.00000f
};

uint8_t constrain_angle(int16_t angle, uint8_t *quadrant){
	//Get the angle into 0..1024 segments (0..360 degrees)
	while (angle < 0){
		angle += 1024;
	}
	while (angle >= 1024){
		angle -= 1024;
	}

	if (angle <= 255) {
		*quadrant = 1;
	}
	else if (angle <= 512){
		*quadrant = 2;
		angle = 512 - angle;
	}
	else if (angle <= 768){
		*quadrant = 3;
		angle = angle - 512;
	}
	else {
		*quadrant = 4;
		angle = 1024 - angle;
	}

	return angle;
}

float acos_f(float value){
	if (value < -1 || value > 1) return NAN;
	return lookup_acos[(uint8_t) ((value * 100) + 100)];
}

float cos_f(float angle_r){
	int16_t angle = angle_r * 512 / M_PI;
	uint8_t quadrant = 0;
	angle = constrain_angle(angle, &quadrant);
	return lookup_cos[angle] * (quadrant == 2 || quadrant == 3 ? -1 : 1);
}

float sin_f(float angle_r){
	int16_t angle = angle_r * 512 / M_PI;
	uint8_t quadrant = 0;
	angle = constrain_angle(angle, &quadrant);
	return lookup_sin[angle] * (quadrant >= 3 ? -1 : 1);
}

uint16_t sqrt_f(uint16_t q){
	uint8_t r;
	uint8_t mask;
	uint8_t i = 8 * sizeof(r) - 1;

	r = mask = 1 << i;

	for (; i > 0; i--){
		mask >>=  1;

		if (q < ( uint16_t ) r * r ) r -= mask;
		else r += mask;
	}

	if (q < (uint16_t) r * r ) r -= 1;

	return r ;
}


//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
// with changes from http://stackoverflow.com/questions/15818906/does-this-pointer-casting-break-strict-aliasing-rule
// to fix a warning about strict aliasing rule being broken
float invSqrt(float x){
	float halfx = 0.5f * x;
	float y = x;
	uint32_t i = (union { float f; uint32_t l; }) {y} .l;
	i = 0x5f3759df - (i>>1);
	y = (union { uint32_t l; float f; }) {i} .f;
	y = y * (1.5f - (halfx * y * y));
	return y;
}
